#include <cstdlib>
#include <cstdio>
#include <ctime>
#include "opencv2/myopencv.hpp"
#include "DepthReader.h"
#include "DepthTracker.h"
#include "GrayReader.h"

using namespace std;

DepthReader depthReader("depth_data.dat", "depth_meta.txt");
GrayReader grayReader("gray_data.dat", "gray_meta.txt");
DepthTracker depthTracker;

void tryHoughCircle(void) {
	int h, w;
	h = grayReader.height;
	w = grayReader.width;

	FILE *outF;
	outF = fopen("gray_center.txt", "w");

	CvMat *gray = cvCreateMat(h, w, CV_8UC1);
	CvMat *last = cvCreateMat(h, w, CV_8UC1);
	CvMat *diff1 = cvCloneMat(gray);
	CvMat *diff2 = cvCloneMat(gray);
	CvMat *diff = cvCloneMat(gray);
	CvMat *edge = cvCloneMat(gray);

	CvMemStorage* storage = cvCreateMemStorage(0);
	while (true) {
		if (grayReader.getNextFrame(gray->data.ptr) == false) break;
		cout << "curID: " << grayReader.curRecId << endl;

		cvShowImage("gray", gray);

// 		cvSub(gray, last, diff1);
// 		cvSub(last, gray, diff2);
// 		cvAdd(diff1, diff2, diff);
// 		cvShowImage("diff", diff);
// 		cvCopy(gray, last);
		cvSmooth( gray, gray, CV_GAUSSIAN, 9, 9 );

		cvCanny(gray, edge, 100, 200);
		cvShowImage("edge", edge);
		CvSeq* circles = cvHoughCircles(gray, storage, CV_HOUGH_GRADIENT,
			1, 80, 100, 40, 5, 30);

		cout << circles->total << endl;
		for(int i=0;i<circles->total;i++)
		{
			float* p=(float*)cvGetSeqElem(circles,i);
// 			cvCircle(gray,cvPoint(cvRound(p[0]),cvRound(p[1])),3,cvScalar(255),-1,8,0);
			cvCircle(gray,cvPoint(cvRound(p[0]),cvRound(p[1])),cvRound(p[2]),cvScalar(255),1,8,0);
			cout << "r: " << cvRound(p[2]) << endl;
		}
		cvShowImage("circles",gray);

		char c = cvWaitKey(0);//1000/30);
		if (c == 13) {
			float* p=(float*)cvGetSeqElem(circles,0);
			fprintf(outF, "%d %d \t %d %d %d\n", grayReader.curRecId, grayReader.curRecClocks,
				cvRound(p[0]), cvRound(p[1]), cvRound(p[2]));
		}
		if (c == 27) break;
	}

	fclose(outF);
	cvReleaseMemStorage(&storage);

	cvReleaseMat(&gray);
	cvReleaseMat(&last);
	cvReleaseMat(&diff1);
	cvReleaseMat(&diff2);
	cvReleaseMat(&diff);
	cvReleaseMat(&edge);
}

void tryDepthTracker(void) {
	CvMat *frame = cvCreateMat(depthReader.height,depthReader.width,CV_16SC1);

	bool firstBlood = true;
	CvRect rect;
	int waitTime = 10;

	FILE *outF = fopen("depth_center.txt", "w");

	while (true) {
		if (depthReader.getNextFrame(frame->data.s) == false) break;
		cout << "curID: " << depthReader.curRecId << endl;
		if (depthReader.curRecId == 1350) break;
		if (firstBlood) {
			firstBlood = false;
			rect.x = 484;
			rect.y = 28;
			rect.height = 518-rect.x;
			rect.width = 60-rect.y;

			depthTracker.initTracking(frame, rect);
		}
		else {

			depthTracker.tracking(frame);
			depthTracker.tracking(frame);

		}

		cvCircle(frame, cvPoint(depthTracker.center.x, depthTracker.center.y), 23,
			cvScalar(32767));

		cvShowImage("depth", frame);

		fprintf(outF, "%d %d \t %d %d %d\n", depthReader.curRecId, depthReader.curRecClocks,
			cvRound(depthTracker.center.x), cvRound(depthTracker.center.y), cvRound(depthTracker.center.z));

		cvWaitKey(waitTime);
	}

	fclose(outF);
	cvReleaseMat(&frame);
}

class CircleFinder {
public:
	struct CircleData {
		int x, y, z, c;
		double rate;
	};
	static const int MAXCIRCLE = 20;
	CircleData circles[MAXCIRCLE];
	int circleIdx;

public:
	int *circumference;
	CvPoint **idxOffset;
	int maxR;
	int *cnt;

public:

	CircleFinder(int maxR = 100) {
		this->maxR = maxR;
		circumference = new int[maxR];
		int size = maxR*2 + 20;
		cnt = new int[maxR];

		CvMat *bigImg;
		bigImg = cvCreateMat(size, size, CV_8UC1);
		uchar **matArr = new uchar *[size];
		for (int i=0; i<size; ++i)
			matArr[i] = cvPtr2D(bigImg, i, 0);
		
		idxOffset = new CvPoint *[maxR];
		for (int r=0; r<maxR; ++r) {
			cvZero(bigImg);
			cvCircle(bigImg, cvPoint(size/2,size/2), r, cvScalar(255));
			circumference[r] = cvCountNonZero(bigImg);
			int k = 0;
			for (int i=0; i<size; ++i)
				for (int j=0; j<size; ++j)
					if (matArr[i][j] != 0)
						++k;
			idxOffset[r] = new CvPoint [k];
			k = 0;
			for (int i=0; i<size; ++i)
				for (int j=0; j<size; ++j)
					if (matArr[i][j] != 0) {
						idxOffset[r][k].x = i-size/2;
						idxOffset[r][k].y = j-size/2;
						++k;
					}
		}
		delete[] matArr;
		cvReleaseMat(&bigImg);
	}

	~CircleFinder(void) {
		delete[] cnt;
		delete[] circumference;
		for (int i=0; i<maxR; ++i)
			delete[] idxOffset[i];
		delete[] idxOffset;
	}

	int findCircles(CvMat *gray, CvRect &subRect, CvPoint3D32f *reslist, int lr, int ur, int offset=0) {
		int w, h;
		w = gray->width;
		h = gray->height;


		uchar **grayArr = new uchar *[h];
		for (int i=0; i<h; ++i)
			grayArr[i] = cvPtr2D(gray, i, 0);

		circleIdx = 0;

		for (int i=max(subRect.y, maxR); i<min(h-maxR, subRect.y+subRect.height); ++i) {
			for (int j=max(subRect.x, maxR); j<min(w-maxR, subRect.x+subRect.width); ++j) {
				for (int k=lr-offset; k<ur+offset; ++k) {
					cnt[k] = 0;
					for (int t=0; t<circumference[k]; ++t) {
						int a, b;
						a = i+idxOffset[k][t].x;
						b = j+idxOffset[k][t].y;
						if (grayArr[a][b] > 10) {
							++cnt[k];
						}
					}
				}

				for (int k=lr; k<ur; ++k) {
					if (circleIdx > MAXCIRCLE) continue;
					if (circleIdx==MAXCIRCLE) {
						++circleIdx;
						cout << "too many circles!!" << endl;
						continue;
					}
					if (cnt[k]>=0.5*circumference[k]) {
						circles[circleIdx].x = i;
						circles[circleIdx].y = j;
						circles[circleIdx].z = k;
						circles[circleIdx].c = cnt[k];
						circles[circleIdx].rate = cnt[k]*1.0/circumference[k];
						++circleIdx;
						
					}
				}
			}
		}

		if (reslist!=NULL) {
			for (int i=0; i<circleIdx; ++i) {
				reslist[i].x = circles[i].x;
				reslist[i].y = circles[i].y;
				reslist[i].z = circles[i].z;
			}
			reslist[circleIdx].x = -1;
			reslist[circleIdx].y = -1;
			reslist[circleIdx].z = -1;
		}
		delete[] grayArr;


// 		cout << "total : " << circleIdx << endl;
// 		CvMat *img1 = cvCreateMat(gray->rows, gray->cols, CV_8UC3);
// 		cvMerge(gray, gray, gray, NULL, img1);
// 		CvMat *img2 = cvCloneMat(img1);
		for (int i=0; i<circleIdx; ++i) {
			cout << circles[i].x << " " << circles[i].y << " " << circles[i].z << " "
				<< circles[i].c << " " << circles[i].rate << endl;
// 			cvCopy(img1, img2);
// 			cvCircle(img2, cvPoint(circles[i].y, circles[i].x), circles[i].z, cvScalar(0,0,255));
// 			char name[100];
// 			sprintf(name, "img%d", i);
// 			cvShowImage(name, img2);
		}
// 		cvReleaseMat(&img1);
// 		cvReleaseMat(&img2);
		return circleIdx;
	}
};

void tryMyCircleFinding(void) {
	const int DETECT_OFFSET = 30;
	const int R_OFFSET = 2;
	int h, w;
	h = grayReader.height;
	w = grayReader.width;

	FILE *outF;
	outF = fopen("gray_center.txt", "w");

	CvMat *gray = cvCreateMat(h, w, CV_8UC1);
	CvMat *edge = cvCloneMat(gray);
	CvMat *binary = cvCloneMat(gray);
	CvMat *color = cvCreateMat(h, w, CV_8UC3);

	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq *contours= NULL;

	CircleFinder circleFinder(30);
	CvPoint3D32f circles[100];

	int expectR = 20;

	CvRect subRect;
	subRect.x = 0;
	subRect.y = 0;
	subRect.height = h;
	subRect.width = w;
	
	while (true) {
		if (grayReader.getNextFrame(gray->data.ptr) == false) break;
		cout << "curID: " << grayReader.curRecId << endl;
		if (grayReader.curRecId == 1350) break;
		int startTime = clock();

		cvShowImage("gray", gray);
		cvMerge(gray, gray, gray, NULL, color);
		
		cvSmooth( gray, gray, CV_GAUSSIAN,5);
		cvShowImage("smooth", gray);
		
		cvCanny(gray, edge, 80, 160);
		cvShowImage("edge", edge);

		cout << "stage one: " << (clock() - startTime + 0.0) / CLOCKS_PER_SEC << endl;

		int total = circleFinder.findCircles(edge, subRect, NULL, expectR-R_OFFSET, expectR+R_OFFSET);
		if (total == 0) {
			subRect.x = 0;
			subRect.y = 0;
			subRect.height = h;
			subRect.width = w;
			total = circleFinder.findCircles(edge, subRect, NULL, expectR-R_OFFSET, expectR+R_OFFSET);
		}
		if (total == 0) {
			cvCanny(gray, edge, 40, 80);
			cvShowImage("edge", edge);
			total = circleFinder.findCircles(edge, subRect, NULL, expectR-R_OFFSET, expectR+R_OFFSET);
		}
		CvPoint3D32f ball;
		ball.x = ball.y = ball.z = 0;

		double sum = 0;
		for (int i=0; i<total; ++i)
			sum += circleFinder.circles[i].rate;

		for (int i=0; i<total; ++i) {
			ball.x += circleFinder.circles[i].rate/sum * circleFinder.circles[i].x;
			ball.y += circleFinder.circles[i].rate/sum * circleFinder.circles[i].y;
			ball.z += circleFinder.circles[i].rate/sum * circleFinder.circles[i].z;
		}

		cout << "final center: " << ball.x << " " << ball.y << " " << ball.z << endl;
		cvCircle(color, cvPoint(cvRound(ball.y), cvRound(ball.x)), cvRound(ball.z), cvScalar(0,0,255));
		cvShowImage("color", color);

		
		expectR = cvRound(ball.z);
		subRect.x = max(0, int(ball.y - ball.z - DETECT_OFFSET));
		subRect.y = max(0, int(ball.x - ball.z - DETECT_OFFSET));
		subRect.width = min(int(ball.y+ball.z + DETECT_OFFSET), w) - subRect.x;
		subRect.height = min(int(ball.x+ball.z + DETECT_OFFSET), h) - subRect.y;
		
		cout << "stage two: " << (clock() - startTime + 0.0) / CLOCKS_PER_SEC << endl;

		fprintf(outF, "%d %d \t %d %d %d\n", grayReader.curRecId, grayReader.curRecClocks,
			cvRound(ball.y), cvRound(ball.x), cvRound(ball.z));

		char c = cvWaitKey(10);
		if (c == 27) break;
	}

	fclose(outF);
	cvReleaseMemStorage(&storage);

	cvReleaseMat(&gray);
	cvReleaseMat(&binary);
	cvReleaseMat(&edge);
}

int main(void)
{		
// 	tryHoughCircle();
// 	tryMyCircleFinding();
	tryDepthTracker();
	return 0;
}